#include "agvwindow.h"
#include "ui_agvwindow.h"
#include <QTimer>
#include <QSerialPort>
#include <QSerialPortInfo>
#include <QDebug>
#include <QPen>
#include <QPointF>
#include <QLineF>

AGVWindow::AGVWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::AGVWindow), x(200), y(200) {
    ui->setupUi(this);

    // 初始化串口通信
    serialPort = new QSerialPort(this);
    serialPort->setBaudRate(QSerialPort::Baud9600);
    serialPort->setDataBits(QSerialPort::Data8);
    serialPort->setParity(QSerialPort::NoParity);
    serialPort->setStopBits(QSerialPort::OneStop);
    serialPort->setFlowControl(QSerialPort::NoFlowControl);

    // 初始化图形场景
    scene = new QGraphicsScene(this);
    ui->graphicsView->setScene(scene);
//    ui->graphicsView->setFixedSize(400, 400); // 设置视图大小
//    scene->setSceneRect(0, 0, 400, 400);  // 设置场景大小

    // 信号与槽连接
    connect(ui->forwardButton, &QPushButton::clicked, this, &AGVWindow::moveForward);
    connect(ui->backwardButton, &QPushButton::clicked, this, &AGVWindow::moveBackward);
    connect(ui->leftButton, &QPushButton::clicked, this, &AGVWindow::turnLeft);
    connect(ui->rightButton, &QPushButton::clicked, this, &AGVWindow::turnRight);
    connect(ui->openPortButton, &QPushButton::clicked, this, &AGVWindow::openSerialPort);
    connect(ui->closePortButton, &QPushButton::clicked, this, &AGVWindow::closeSerialPort);

    // 定时器用于更新状态
    timer = new QTimer(this);
    connect(timer, &QTimer::timeout, this, &AGVWindow::updateStatus);
    timer->start(1000); // 每秒更新一次状态

    // 添加初始位置到轨迹点
    trajectoryPoints.append(QPointF(x, y));
    updateTrajectory();
}

AGVWindow::~AGVWindow() {
    delete ui;
    if (serialPort->isOpen()) {
        serialPort->close();
    }
}

void AGVWindow::updateStatus() {
    ui->statusLabel->setText(QString("AGV Status: Idle at (%1, %2)").arg(x).arg(y));
}

void AGVWindow::moveForward() {
    y -= 10;
    ui->statusLabel->setText(QString("AGV Status: Moving Forward to (%1, %2)").arg(x).arg(y));
    if (serialPort->isOpen()) {
        serialPort->write("F"); // 向STM32发送前进指令
    }
    trajectoryPoints.append(QPointF(x, y));
    updateTrajectory();
}

void AGVWindow::moveBackward() {
    y += 10;
    ui->statusLabel->setText(QString("AGV Status: Moving Backward to (%1, %2)").arg(x).arg(y));
    if (serialPort->isOpen()) {
        serialPort->write("B"); // 向STM32发送后退指令
    }
    trajectoryPoints.append(QPointF(x, y));
    updateTrajectory();
}

void AGVWindow::turnLeft() {
    x -= 10;
    ui->statusLabel->setText(QString("AGV Status: Turning Left to (%1, %2)").arg(x).arg(y));
    if (serialPort->isOpen()) {
        serialPort->write("L"); // 向STM32发送左转指令
    }
    trajectoryPoints.append(QPointF(x, y));
    updateTrajectory();
}

void AGVWindow::turnRight() {
    x += 10;
    ui->statusLabel->setText(QString("AGV Status: Turning Right to (%1, %2)").arg(x).arg(y));
    if (serialPort->isOpen()) {
        serialPort->write("R"); // 向STM32发送右转指令
    }
    trajectoryPoints.append(QPointF(x, y));
    updateTrajectory();
}

void AGVWindow::readSerialData() {
    QByteArray data = serialPort->readAll();
    qDebug() << "Received data from STM32:" << data;
    // 根据接收到的数据更新界面或状态
}

void AGVWindow::openSerialPort() {
    QString portName = ui->serialPortLineEdit->text();
    serialPort->setPortName(portName);
    if (serialPort->open(QIODevice::ReadWrite)) {
        qDebug() << "Serial port opened successfully!";
        connect(serialPort, &QSerialPort::readyRead, this, &AGVWindow::readSerialData);
    } else {
        qDebug() << "Failed to open serial port!";
    }
}

void AGVWindow::closeSerialPort() {
    if (serialPort->isOpen()) {
        serialPort->close();
        qDebug() << "Serial port closed successfully!";
    }
}

void AGVWindow::updateTrajectory() {
    // 清除场景中的所有项
    scene->clear();

    // 绘制轨迹
    QPen pen(Qt::blue, 2);
    for (int i = 0; i < trajectoryPoints.size() - 1; ++i) {
        scene->addLine(QLineF(trajectoryPoints[i], trajectoryPoints[i + 1]), pen);
    }

    // 绘制当前AGV小车的位置（用一个小圆圈表示）
    QBrush brush(Qt::red);
    scene->addEllipse(x - 5, y - 5, 10, 10, pen, brush);
}
